
#include <agv_peripheral_controller/ioLiftDoubleSteer/peripheral_lift.h>
#include <agv_peripheral_controller/toolKit.h>

#define ORDER_UP "pick"	  // 动作上升指令
#define ORDER_DOWN "drop" // 动作下降指令

#define PRE_TYPE "lhdAdjust" // 预提升动作
#define PRE_HEIGHT "height"

#define OP_TIME_OUT (35 * 1000) // 35s
namespace agv_peripheral
{

	// 默认构造函数
	CPeripheral::CPeripheral(const std::string &actonName)
		: m_server(m_priNh, actonName, false)
	{
		// 注册回调函数
		m_server.registerGoalCallback(boost::bind(&CPeripheral::goalCB, this));
		m_server.registerPreemptCallback(boost::bind(&CPeripheral::preemptCB, this));
	}

	// 析构函数
	CPeripheral::~CPeripheral()
	{
		m_server.shutdown();
	}

	// 初始化函数
	bool CPeripheral::init()
	{
		bool ret = false;

		// 加载参数
		loadParam();

		// 举升设备初始化
		ret = m_lift.init();

		// 服务端开启
		m_server.start();

		if (!ret)
		{
			DEBUG_ERROR_OUT("CPeripheral init failed");
		}

		m_event.init(m_priNh);

		return ret;
	}

	// 加载参数和订阅话题
	void CPeripheral::loadParam()
	{
		// m_eventPub = m_priNh.advertise<agv_msgs::msg::Event>("/event", 1);
		m_periStatusPub = m_priNh.advertise<agv_msgs::msg::PeriphStatus>("/periphStatus", 1);
		m_periTelopSub = m_priNh.subscribe("/base/periphTeleopInfo", 1, &CPeripheral::periphTeleopCtlCB, this);
		m_agvInfoSub = m_priNh.subscribe("/agvmode", 1, &CPeripheral::agvInfoCB, this);
		m_curMode = agv_msgs::msg::AGVMode::MANAUL; // LOCAL_AUTOMATIC
		m_curStage = STAGE_IDLE;
	}

	// 复位相关变量和状态
	void CPeripheral::reset()
	{
		m_errorMsg = "";
		eventPub(false, "");
		m_curStage = STAGE_IDLE;
		m_manulOpCode.Update(NO_OP);
		m_autoOpCode.Update(NO_OP);
		execOp(NO_OP);
	}

	void CPeripheral::goalCB()
	{
		std::lock_guard<std::mutex> locker(m_modeMtx);

		// #1当前服务端未激活或者未有新的可用的goal
		// bool active = m_server.isActive() ; //此时未激活
		// DEBUG_WARN_OUT("new goal CB:active="<<active<<",valid="<<valid);
		bool valid = m_server.isNewGoalAvailable();
		DEBUG_WARN_OUT("new goal CB:="
					   << ",valid=" << valid);
		if (!valid)
		{
			DEBUG_ERROR_OUT("CPeripheral::goalCB is not active or not available");
			return;
		}

		// #2 当前不是自动模式，返回错误
		if (m_curMode != agv_msgs::msg::AGVMode::LOCAL_AUTOMATIC && m_curMode != agv_msgs::msg::AGVMode::AUTOMATIC)
		{
			setResult(FAILED, "current is not auto mode;");
			DEBUG_ERROR_OUT("goalCB:cur mode is not auto mode ,goal can not exec,m_curMode=" << m_curMode);
			return;
		}

		// #3 当前阶段不是空闲阶段
		if (m_curStage != STAGE_IDLE)
		{
			setResult(FAILED, "current is not idle");
			DEBUG_ERROR_OUT("CPeripheral::goalCB;curStage is not stage_idle;m_curStage=" << m_curStage);
			return;
		}

		// #4 解析当前动作
		m_curAction = m_server.acceptNewGoal()->goal_action;
		if (m_curAction.actionType == ORDER_UP)
		{
			m_autoOpCode.Update(UP);
			m_curStage = STAGE_RUN;
			setFeedBack(UP, 0.0f);
			DEBUG_WARN_OUT("goal: start up");
		}
		else if (m_curAction.actionType == ORDER_DOWN)
		{
			m_autoOpCode.Update(DOWN);
			m_curStage = STAGE_RUN;
			setFeedBack(DOWN, 0.0f);
			DEBUG_WARN_OUT("goal: start down");
		}
		else if (m_curAction.actionType == PRE_TYPE)
		{
			auto params = m_curAction.actionParameters;

			for (auto param : params)
			{
				if (param.key == PRE_HEIGHT)
				{
					double fHeight = 0.0f;
					if (string2Double(param.value, fHeight))
					{
						OpCode code = fHeight > 0.5 ? UP : DOWN;
						m_autoOpCode.Update(code);
						m_curStage = STAGE_RUN;
						setFeedBack(code, 0.0f);
						DEBUG_WARN_OUT("goal: start code=" << (int)(code));
					}
					return;
				}
			}

			DEBUG_ERROR_OUT("goal: error no PRE_HEIGHT");
		}
		else
		{
			m_autoOpCode.Update(NO_OP);
			m_curStage = STAGE_IDLE;
			DEBUG_WARN_OUT("goal: no op");
			setResult(FAILED, "current op in unkown;can not exec");
			return;
		}
	}

	// 抢占式回调函数
	void CPeripheral::preemptCB()
	{
		std::lock_guard<std::mutex> locker(m_modeMtx);
		// #1 取消当前动作 #2 停止举升 #3复位状态
		// if (m_server.isActive())
		// {
		m_lift.stop();
		DEBUG_WARN_OUT("CPeripheral cancel current action:stop lift");
		// }
		// #2 取消任务 复位状态
		reset();
		setPreempt(CANCELED, "action  has been  canceled");
	}

	// 操作模式回调函数
	void CPeripheral::agvInfoCB(const agv_msgs::msg::AGVMode::ConstSharedPtr &msg)
	{

		uint8_t newWorkMode = msg->mode;
		// input_status.manualState = (state == agv_msgs::msg::AGVMode::MANAUL);
		// #1 切换手自动等模式
		if (m_curMode != newWorkMode)
		{
			std::lock_guard<std::mutex> locker(m_modeMtx);
			m_curMode = newWorkMode;
			reset();
			DEBUG_OUT("CPeripheral:change to newMode:" << newWorkMode << ",last mode:" << m_curMode);
		}
	}

	// 外设手柄操作回调函数
	void CPeripheral::periphTeleopCtlCB(const agv_msgs::msg::PeriphTeleop::ConstSharedPtr &msg)
	{
		std::lock_guard<std::mutex> locker(m_modeMtx);

		if (m_curMode != agv_msgs::msg::AGVMode::MANAUL)
		{
			DEBUG_ERROR_OUT("periphTeleopCtlCB:cur mode is not  manul ,can not to periphTeleopCtlcb");
			return;
		}

		uint8_t order = msg->order;
		uint32_t freq = msg->freq;
		if (!freq)
		{
			DEBUG_WARN_OUT("periphTeleopCtlCB: freg is not valid:freq=" << freq);
			return;
		}

		if (order == UP)
		{
			m_manulOpCode.Update(UP);
		}
		else if (order == DOWN)
		{
			m_manulOpCode.Update(DOWN);
		}
	}

	// 查看发布者是否有效(初始化)
	bool CPeripheral::publiserValid(const ros::Publisher &pub)
	{
		uint32_t num = pub.getNumSubscribers();
		bool valid = num > 0;
		return valid;
	}

	// 外设状态发布
	void CPeripheral::periphStatusPub()
	{
		// 如果发布者无效
		if (!publiserValid(m_periStatusPub))
		{
			DEBUG_ERROR_OUT("m_periStatusPub is invalid");
			return;
		}

		// 发布外设的桩体
		agv_msgs::msg::PeriphStatus status;
		status.header.stamp = rclcpp::Time::now();

		// 当前的载货状态
		agv_msgs::msg::V_load load;

		// 可能需要上报外设是否初始化完成 待修改
		status.periphStateInfo = -1;
		load.loadType = std::to_string(m_lift.loadStatus());	   // 是否设备举起还是在最低位
		load.loadPosition = std::to_string(m_lift.loadDetector()); // 是否真正有无货物
		status.loaded = m_lift.loadDetector();					   // 作用？
		status.loads.push_back(load);

		m_periStatusPub.publish(status);
	}

	// 外设事件发布
	void CPeripheral::eventPub(bool occur, const std::string &msg)
	{
		if (occur)
		{
			m_event.updateEvent(agv_msgs::msg::AGVEvent::LOAD_AND_UNLOAD_ERROR, msg);
		}
		else if (m_event.checkEvent(agv_msgs::msg::AGVEvent::LOAD_AND_UNLOAD_ERROR))
		{
			m_event.removeEvent(agv_msgs::msg::AGVEvent::LOAD_AND_UNLOAD_ERROR);
		}
	}

	// 设置实时运行反馈
	void CPeripheral::setFeedBack(OpCode code, float percent)
	{
		m_actionFeedBack.state = code;
		m_actionFeedBack.percent_complete = percent;
		m_actionFeedBack.feedback_action = m_curAction;
		m_server.publishFeedback(m_actionFeedBack);
	}

	// 设置结果返回
	void CPeripheral::setResult(OpResult result, std::string msg)
	{
		m_actionResult.success = result;
		m_actionResult.message = msg;
		m_actionResult.result_action = m_curAction;
		m_server.setSucceeded(m_actionResult);
	}

	// 设置取消任务反馈
	void CPeripheral::setPreempt(OpResult result, std::string msg)
	{
		m_actionResult.success = result;
		m_actionResult.message = msg;
		m_actionResult.result_action = m_curAction;
		m_server.setPreempted(m_actionResult);
	}

	// 举升到高位后-货物检测(如果有货物检测开关)
	int CPeripheral::upAutoExec(int code)
	{
		// 当前不是自动运行阶段
		if (m_curMode != agv_msgs::msg::AGVMode::LOCAL_AUTOMATIC && m_curMode != agv_msgs::msg::AGVMode::AUTOMATIC)
		{
			DEBUG_ERROR_OUT("error:autorun current is not auto mode");
			return UpExeStage::EXE_INVALID;
		}

		// 当前不是自动上升指令
		if (code != UP)
		{
			DEBUG_ERROR_OUT("error:autorun current is not up");
			return UpExeStage::EXE_INVALID;
		}

		// 自动上升-并检测是否到达上限位时，取到货物
		if (execOp(code))
		{
			// 到达上限位，并检测到货物报正常完成
			if (m_lift.loadDetector())
			{
				setResult(SUCCESS, "current Up exec finish");
				m_curStage = STAGE_END;
				DEBUG_WARN_OUT("enter stage end;current code=" << code << " is exec ok;finish");
				return UpExeStage::EXE_FINISHED;
			}
			else
			{
				// 到达上限位，未检测到货物-失败
				if (execOp(NO_OP))
				{
					m_curStage = STAGE_FAILED;
					DEBUG_ERROR_OUT("error:enter stage_failed; op up exec failed loadDetector is false");
				}
				m_errorMsg = "code up exec failed;no loads  io signal found";
				eventPub(true, m_errorMsg);
				DEBUG_ERROR_OUT(m_errorMsg);
				return UpExeStage::EXE_FAILED;
			}
		}
		else
		{
			// 如果执行30秒还未完成,停止动作再进入超时报错
			bool orderExeTimeOut = m_autoOpCode.TimeOut(OP_TIME_OUT);
			if (orderExeTimeOut)
			{
				if (execOp(NO_OP))
				{
					m_curStage = STAGE_FAILED;
					DEBUG_ERROR_OUT("error:enter stage_failed; op up exec time out");
				}

				m_errorMsg = "code up exec time out > 30s";
				eventPub(true, m_errorMsg);
				DEBUG_ERROR_OUT(m_errorMsg);
				return UpExeStage::EXE_FAILED;
			}
		}

		return UpExeStage::EXE_RUNNING;
	}

	// 自动运行模式
	void CPeripheral::autoRun()
	{

		if (m_curMode != agv_msgs::msg::AGVMode::LOCAL_AUTOMATIC && m_curMode != agv_msgs::msg::AGVMode::AUTOMATIC)
		{
			DEBUG_ERROR_OUT("error:autorun current is not auto mode");
			return;
		}
		// 自动运行中，复位手动控制状态
		m_lift.manulSwitchIoLift(true);

		m_lift.manulSwitchNormalLift(true);

		bool emg = m_lift.emgPressed();
		if (emg)
		{
			DEBUG_OUT("autoRun emg pressed");
			execOp(NO_OP);
			if (STAGE_RUN == m_curStage)
			{
				if (!m_autoOpCode.TimeOut(OP_TIME_OUT))
				{
					m_autoOpCode.UpdateTime();
					DEBUG_OUT("autoRun emg pressed update time");
				}
			}
			return;
		}

		int code = m_autoOpCode.Get();
		switch (m_curStage)
		{
		case STAGE_IDLE:
		{
			static int times = 0;
			if (times++ > 40)
			{
				// 自动运行中，防止上一次有指令，举升动作，强制停止一次
				execOp(NO_OP);
				times = 0;
				DEBUG_OUT("stage idle wait a new code...");
			}
		}
		break;
		case STAGE_RUN:
		{
			// 上升抬货特殊检测保护
			if (code == UP)
			{
				upAutoExec(code);
			}
			else if (execOp(code))
			{
				setResult(SUCCESS, "current exec finish");
				m_curStage = STAGE_END;
				DEBUG_WARN_OUT("enter stage end;current code=" << code << " is exec ok;finish");
			}
			else
			{
				// 如果执行30秒还未完成,停止动作再进入超时报错
				bool orderExeTimeOut = m_autoOpCode.TimeOut(OP_TIME_OUT);
				if (orderExeTimeOut)
				{
					if (execOp(NO_OP))
					{
						m_curStage = STAGE_FAILED;
						DEBUG_ERROR_OUT("error:enter stage_failed; op exec time out");
					}
					m_errorMsg = "code exec time out > 30s";
					eventPub(true, m_errorMsg);
					DEBUG_ERROR_OUT(m_errorMsg);
				}
			}
		}
		break;
		case STAGE_END:
		{
			if (execOp(NO_OP))
			{
				m_curStage = STAGE_IDLE;
				DEBUG_WARN_OUT("enter stage idle; clear m_autoOpCode");
			}
			m_autoOpCode.Update(NO_OP);
		}
		break;

		case STAGE_FAILED:
		{
			eventPub(true, m_errorMsg);
			DEBUG_OUT("stage failed..clear m_autoOpCode");
			m_autoOpCode.Update(NO_OP);
		}
		break;

		default:
			break;
		}
	}

	// 手动运行模式
	void CPeripheral::manulRun()
	{
		if (m_curMode != agv_msgs::msg::AGVMode::MANAUL)
		{
			DEBUG_ERROR_OUT("error:manulRun current is not manul mode");
			return;
		}

		bool emg = m_lift.emgPressed();
		if (emg)
		{
			DEBUG_OUT("manul emg pressed");
			execOp(NO_OP);
			return;
		}

		if (!m_lift.manulSwitchIoLift())
		{
			m_lift.stopLeftRight();
		}

		if (!m_lift.manulSwitchNormalLift())
		{
			//  m_lift.stop();
			bool timeOut = m_manulOpCode.TimeOut(400);
			if (timeOut)
			{
				execOp(NO_OP);
				DEBUG_OUT("manulRun: order time out;set stop");
				return;
			}

			int code = m_manulOpCode.Get();
			execOp(code);
		}

		// 如果配置了三挡开关，pc调试软件失效
		/*
		  bool timeOut = m_manulOpCode.TimeOut(100);
		  if (timeOut)
		  {
			  execOp(NO_OP);
			  DEBUG_WARN_OUT("manulRun: order time out;set stop");
			  return;
		  }

		  int code = m_manulOpCode.Get();
		  execOp(code);
		  */
	}

	// 其他操作模式运行
	void CPeripheral::otherRun()
	{
		execOp(NO_OP);
	}

	// 执行操作
	bool CPeripheral::execOp(int code)
	{
		bool finish = false;
		switch (code)
		{
		case UP:
			finish = m_lift.up();
			DEBUG_OUT("op lift up:finish=" << finish);
			break;
		case DOWN:
			finish = m_lift.down();
			DEBUG_OUT("op lift donw:finish=" << finish);
			break;
		default:
			finish = m_lift.stop();
			DEBUG_OUT("other op lift stop:finish=" << finish);
			break;
		}
		return finish;
	}

	// 运行函数
	void CPeripheral::running()
	{
		std::lock_guard<std::mutex> locker(m_modeMtx);

		switch (m_curMode)
		{
		case agv_msgs::msg::AGVMode::MANAUL:
			manulRun();
			break;
		case agv_msgs::msg::AGVMode::LOCAL_AUTOMATIC:
		case agv_msgs::msg::AGVMode::AUTOMATIC:
			autoRun();
			break;
		default:
			otherRun();
			break;
		}

		periphStatusPub();
	}
}; // end namespace

int main(int argc, char **argv)
{
	setlocale(LC_ALL, "");
	rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("agv_peripheral_controller");
	agv_peripheral::CPeripheral peripheralObj("do_peripheral");
	bool initOk = peripheralObj.init();
	DEBUG_OUT("peripheralObj:initOk=" << initOk);

	// ros::AsyncSpinner spinner(4);
	// spinner.start();

	rclcpp::Rate rate(20);
	while (rclcpp::ok())
	{
		peripheralObj.running();

		rclcpp::spin_some(node);
		rate.sleep();
	}

	return 0;
}
